/*
 * MSFollowLeader.java
 *
 * Created on 7 de noviembre de 2007, 11:19 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Behaviors.Schemas.Motor;
import Behaviors.Schemas.Perception.PSDetectLeader;
import Behaviors.Schemas.Schema;
import Behaviors.Schemas.Vector2dSchema;
import Robot.*;
import Util.Vector2d;
import Util.Pose2d;
/**
 *
 * @author jonathan
 */
public class MSFollowLeader extends Schema implements Vector2dSchema{
    
    static final float MAX_DIST_PROP=2.0f;
    static final float MIN_DIST_TO_LEADER =40.0f;
    static final float MAX_VEL = 1.0f;
    Position2d myPosition;
    PSDetectLeader leaderPosition;
    
    
    /**
     * Creates a new instance of MSFollowLeader
     */
    public MSFollowLeader(String name,Position2d myPosition, PSDetectLeader leaderPosition) {
        super(name);
        this.myPosition=myPosition;
        this.leaderPosition=leaderPosition;
    }
    
    public Vector2d Value() {
        
        Pose2d myPose = myPosition.read(RealPosition2d.READ_MODE_NO_WAIT);
        
        
        Vector2d myPos=new Vector2d(myPose.getPx(),myPose.getPy(),Vector2d.MODE_XY);
        
        //Get leader position from Perceptual Schema
        Vector2d leaderPos=leaderPosition.Value();
        
        Vector2d atractor=leaderPos.sub(myPos);
        
        //if robot is beyond of minimal distance to leader
        if(atractor.getMag()>MIN_DIST_TO_LEADER)
            //If it is far away, max vel
            if(atractor.getMag()>MAX_DIST_PROP){
                atractor.normalize();
                return atractor.scale(MAX_VEL);
            }
            else
                //if it is near, scale vel according to distance
                return atractor.scale(1/MAX_DIST_PROP);
        else
            //robot is in the required position, it is inside the sphere of proximity
            return new Vector2d(0,0,Vector2d.MODE_XY);

    }
    
}
